#include "ref_navigator.h"


ref_navigator_c::
ref_navigator_c(float period)
{
	delta_x 		= 0;
	delta_y 		= 0;
	delta_z			= 0;
	x_speed 		= 0; 
	y_speed 		= 0;
	z_speed			= 0;
	mission_time_h	= 0;
	mission_time_v 	= 0;
	startpoint.x    = 0;
	startpoint.y    = 0;
	startpoint.z    = 0;
	endpoint.x      = 0;
	endpoint.y      = 0;
	endpoint.z      = 0;
	dT				= period;
	flag_x			= 0;
	flag_y			= 0;
	flag_z			= 0; 
}

ref_navigator_c::
ref_navigator_c()
{
	delta_x 		= 0;
	delta_y 		= 0;
	delta_z			= 0;
	x_speed 		= 0; 
	y_speed 		= 0;
	z_speed			= 0;
	mission_time_h	= 0;
	mission_time_v 	= 0;
	startpoint.x    = 0;
	startpoint.y    = 0;
	startpoint.z    = 0;
	endpoint.x      = 0;
	endpoint.y      = 0;
	endpoint.z      = 0;
	dT				= 1;
	flag_x			= 0;
	flag_y			= 0;
	flag_z			= 0; 	
}


ref_navigator_c::
~ref_navigator_c()
{}



void 
ref_navigator_c::
horizontal_generate()
{
	if ((fabs(endpoint.x - endpoint_pre.x) > RANGE_UPDATE) || (fabs(endpoint.y - endpoint_pre.y) > RANGE_UPDATE))
	{
		reset(1, 0);
	}
	
	delta_x = endpoint.x - startpoint.x;
	delta_y = endpoint.y - startpoint.y;

	//printf("startpoint:%f, %f\n", startpoint.x, startpoint.y);
	//printf("endpoint:%f,%f,delta_x = %.2f, delta_y = %.2f\n", endpoint.x, endpoint.y, delta_x, delta_y);

	if ((delta_x != 0) || (delta_y != 0))
	{
		if (direction == HORIZONTAL_NAVIGATOR)
		{
			x_speed = refspeed * delta_x / (sqrt(pow(delta_x, 2) + pow(delta_y, 2)));
			y_speed = refspeed * delta_y / (sqrt(pow(delta_x, 2) + pow(delta_y, 2)));
		}
		else
		{
			x_speed = 1 * delta_x / (sqrt(pow(delta_x, 2) + pow(delta_y, 2)));
			y_speed = 1 * delta_y / (sqrt(pow(delta_x, 2) + pow(delta_y, 2))); 
		}
		
		//printf("x_speed = %.2f, y_speed = %.2f\n", x_speed, y_speed);

		mission_time_h += dT;
		
		ref_point.position.x = startpoint.x + x_speed * mission_time_h;
		ref_point.position.y = startpoint.y + y_speed * mission_time_h;
		//printf("mission_time_h = %.2f\n", mission_time_h);

		if (delta_x >= 0)
		{
			if (ref_point.position.x > endpoint.x)
			{
				ref_point.position.x = endpoint.x;
				flag_x = 1;
			}				
		}
		else
		{
			if (ref_point.position.x < endpoint.x)
			{
				ref_point.position.x = endpoint.x;
				flag_x = 1;
			}				
		}

		if (delta_y >= 0)
		{
			if (ref_point.position.y > endpoint.y)
			{
				ref_point.position.y = endpoint.y;
				flag_y = 1;
			}				
		}
		else
		{
			if (ref_point.position.y < endpoint.y)
			{
				ref_point.position.y = endpoint.y;
				flag_y = 1;
			}				
		}		

		if ((flag_x == 1) && (flag_y == 1))
		{
			mission_time_h -= dT;
		}
		
		ref_point.yaw = atan2(y_speed, x_speed);

		startpoint_pre.x = startpoint.x;
		startpoint_pre.y = startpoint.y;
		
		endpoint_pre.x = endpoint.x;
		endpoint_pre.y = endpoint.y;
		
	}
	if (delta_x == 0)
	{
		ref_point.position.x = endpoint.x;
	}
	if (delta_y == 0)
	{
		ref_point.position.y = endpoint.y;
	}
	
}


void
ref_navigator_c::
vertical_generate()
{
	if (fabs(endpoint.z - endpoint_pre.z) > RANGE_UPDATE)
	{
		reset(0, 1);
	}
	delta_z = endpoint.z - startpoint.z;
	//printf("delta_z = %.2f, ", delta_z);
	//printf("startpoint.z = %.3f, endpoint.z = %.3f, delta_z = %.3f\n", startpoint.z, endpoint.z, delta_z);
	if (delta_z == 0)
	{
		ref_point.position.z = endpoint.z;
	}
	else
	{
		if (direction == VERTICAL_NAVIGATOR)
		{
			z_speed = refspeed * delta_z / fabs(delta_z);
		}
		else
		{
			z_speed = refspeed * delta_z / (sqrt(pow(delta_x, 2) + pow(delta_y, 2)));
		}			

		
		mission_time_v += dT;
		//printf("z_speed = %.2f, mission_time_v = %.3f\n", z_speed, mission_time_v);
		ref_point.position.z = startpoint.z + z_speed * mission_time_v;		

		if (delta_z >= 0)
		{
			if (ref_point.position.z > endpoint.z)
			{
				ref_point.position.z = endpoint.z;
				flag_z = 1;
			}				
		}
		else
		{
			if (ref_point.position.z < endpoint.z)
			{
				ref_point.position.z = endpoint.z;
				flag_z = 1;
			}				
		}

		if (flag_z)
		{
			mission_time_v -= dT;
		}
		//printf("ref_point.position.z = %.2f\n", ref_point.position.z);
		startpoint_pre.z = startpoint.z;
		endpoint_pre.z = endpoint.z;
	}
	
}


void 
ref_navigator_c::
d3_generate()
{	
	horizontal_generate();
	vertical_generate();	
}

void 
ref_navigator_c::
reset(int rh, int rv)
{
	if (rh)
	{
		mission_time_h = 0;
		flag_x = 0;
		flag_y = 0;
	}
	if (rv)
	{
		mission_time_v = 0;
		flag_z = 0;
	}
}


